/*
 * user_presence_app.c
 *
 *  Created on: 2023年4月26日
 *      Author: Administrator
 */


// Copyright (c) Acconeer AB, 2022-2023
// All rights reserved
// This file is subject to the terms and conditions defined in the file
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
// of this source code package.

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "acc_definitions_a121.h"
#include "acc_definitions_common.h"
#include "acc_detector_presence.h"
#include "acc_hal_definitions_a121.h"
#include "acc_hal_integration_a121.h"
#include "acc_integration.h"
#include "acc_rss_a121.h"
#include "acc_sensor.h"

#include "acc_version.h"

#include "user_app.h"

A121_presence_config_t  A121_presence_config;


/** \example example_detector_presence.c
 * @brief This is an example on how the Detector Presence API can be used
 * @n
 * The example executes as follows:
 *   - Create a presence configuration
 *   - Create a sensor instance
 *   - Create a detector instance
 *   - Calibrate the sensor
 *   - Prepare the detector
 *   - Perform a sensor measurement and read out the data
 *   - Process the measurement and get detector result
 *   - Destroy the configuration
 *   - Destroy the detector instance
 *   - Destroy the sensor instance
 */


typedef enum
{
	PRESENCE_PRESET_CONFIG_NONE = 0,
	PRESENCE_PRESET_CONFIG_SHORT_RANGE,
	PRESENCE_PRESET_CONFIG_MEDIUM_RANGE,
	PRESENCE_PRESET_CONFIG_LONG_RANGE,
} presence_preset_config_t;


#define SENSOR_ID         (1U)
#define SENSOR_TIMEOUT_MS (1000U)

#define DEFAULT_PRESET_CONFIG PRESENCE_PRESET_CONFIG_MEDIUM_RANGE

static void presence_print_result(acc_detector_presence_result_t result);


static void presence_cleanup(acc_detector_presence_handle_t *presence_handle,
                    acc_detector_presence_config_t *presence_config,
                    acc_sensor_t                   *sensor,
                    void                           *buffer);


static void presence_set_config(acc_detector_presence_config_t *presence_config, presence_preset_config_t preset);


int user_detector_presence(int argc, char *argv[]);


int user_detector_presence(int argc, char *argv[])
{
	(void)argc;
	(void)argv;
	acc_detector_presence_config_t   *presence_config = NULL;
	acc_detector_presence_handle_t   *presence_handle = NULL;
	acc_detector_presence_metadata_t metadata;
	acc_sensor_t                     *sensor     = NULL;
	void                             *buffer     = NULL;
	uint32_t                         buffer_size = 0U;

	if(A121_presence_config.run_status==0){return 0;}
	printf("Acconeer software version %s\n", acc_version_get());

	const acc_hal_a121_t *hal = acc_hal_rss_integration_get_implementation();

	if (!acc_rss_hal_register(hal))
	{
		return EXIT_FAILURE;
	}

	presence_config = acc_detector_presence_config_create();
	if (presence_config == NULL)
	{
		printf("acc_detector_presence_config_create() failed\n");
		presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	presence_set_config(presence_config, DEFAULT_PRESET_CONFIG);

	// Print the configuration
	acc_detector_presence_config_log(presence_config);

	presence_handle = acc_detector_presence_create(presence_config, &metadata);
	if (presence_handle == NULL)
	{
		printf("acc_detector_presence_create() failed\n");
		presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	if (!acc_detector_presence_get_buffer_size(presence_handle, &buffer_size))
	{
		printf("acc_detector_presence_get_buffer_size() failed\n");
		//presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	buffer = acc_integration_mem_alloc(buffer_size);
	if (buffer == NULL)
	{
		printf("buffer allocation failed\n");
		presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	acc_hal_integration_sensor_supply_on(SENSOR_ID);
	acc_hal_integration_sensor_enable(SENSOR_ID);

	sensor = acc_sensor_create(SENSOR_ID);
	if (sensor == NULL)
	{
		printf("acc_sensor_create() failed\n");
		presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	bool             status;
	bool             cal_complete = false;
	acc_cal_result_t cal_result;

	do
	{
		status = acc_sensor_calibrate(sensor, &cal_complete, &cal_result, buffer, buffer_size);

		if (status && !cal_complete)
		{
			status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
		}
	} while (status && !cal_complete);

	if (!status)
	{
		printf("acc_sensor_calibrate() failed\n");
		presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	// Reset sensor after calibration by disabling it
	acc_hal_integration_sensor_disable(SENSOR_ID);
	acc_hal_integration_sensor_enable(SENSOR_ID);

	if (!acc_detector_presence_prepare(presence_handle, presence_config, sensor, &cal_result,
	                                   buffer, buffer_size))
	{
		printf("acc_detector_presence_prepare() failed\n");
		presence_cleanup(presence_handle, presence_config, sensor, buffer);
		return EXIT_FAILURE;
	}

	while (A121_presence_config.run_status==1)
	{
		acc_detector_presence_result_t result;

		if (!acc_sensor_measure(sensor))
		{
			printf("acc_sensor_measure failed\n");
			presence_cleanup(presence_handle, presence_config, sensor, buffer);
			return EXIT_FAILURE;
		}

		if (!acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS))
		{
			printf("Sensor interrupt timeout\n");
			presence_cleanup(presence_handle, presence_config, sensor, buffer);
			return EXIT_FAILURE;
		}

		if (!acc_sensor_read(sensor, buffer, buffer_size))
		{
			printf("acc_sensor_read failed\n");
			presence_cleanup(presence_handle, presence_config, sensor, buffer);
			return EXIT_FAILURE;
		}

		if (!acc_detector_presence_process(presence_handle, buffer, &result))
		{
			printf("acc_detector_presence_process failed\n");
			presence_cleanup(presence_handle, presence_config, sensor, buffer);
			return EXIT_FAILURE;
		}

		presence_print_result(result);
		HAL_Delay(A121_presence_config.data_output_time);
		A121_app_save_data();
	}

	presence_cleanup(presence_handle, presence_config, sensor, buffer);

	printf("Application finished OK\n");

	return EXIT_SUCCESS;
}


static void presence_cleanup(acc_detector_presence_handle_t *presence_handle,
                    acc_detector_presence_config_t *presence_config,
                    acc_sensor_t                   *sensor,
                    void                           *buffer)
{
	acc_hal_integration_sensor_disable(SENSOR_ID);
	acc_hal_integration_sensor_supply_off(SENSOR_ID);

	if (presence_config != NULL)
	{
		acc_detector_presence_config_destroy(presence_config);
	}

	if (presence_handle != NULL)
	{
		acc_detector_presence_destroy(presence_handle);
	}

	if (sensor != NULL)
	{
		acc_sensor_destroy(sensor);
	}

	if (buffer != NULL)
	{
		acc_integration_mem_free(buffer);
	}
}


static void presence_print_result(acc_detector_presence_result_t result)
{
	if (result.presence_detected)
	{
		printf("Motion  ");
	}
	else
	{
		printf("No motion  ");
	}

	// Score and distance are multiplied by 1000 to avoid printing floats
	printf("Distance=%5dmm,Intra_score=%5d, Inter_score=%5d\n",
			(int)(result.presence_distance * 1000.0f),
	       (int)(result.intra_presence_score * 1000.0f),
	       (int)(result.inter_presence_score * 1000.0f));
}


static void presence_set_config(acc_detector_presence_config_t *presence_config, presence_preset_config_t preset)
{

	acc_detector_presence_config_start_set(presence_config, A121_presence_config.start);
	acc_detector_presence_config_end_set(presence_config, A121_presence_config.end);

	if(A121_presence_config.step_length==0)
	{
		acc_detector_presence_config_auto_step_length_set(presence_config, true);//自动分配步长
	}
	else
	{
		acc_detector_presence_config_auto_step_length_set(presence_config, false);//自动分配步长
		acc_detector_presence_config_step_length_set(presence_config, A121_presence_config.step_length);//步长
	}


	if(A121_presence_config.profile==0)
	{
		acc_detector_presence_config_auto_profile_set(presence_config, true);//自适应profile
	}
	else
	{
		acc_detector_presence_config_auto_profile_set(presence_config, false);//自适应profile
		acc_detector_presence_config_profile_set(presence_config, A121_presence_config.profile);//profile
	}

	if(A121_presence_config.frame_idle<3)
	{
		acc_detector_presence_config_inter_frame_idle_state_set(presence_config, A121_presence_config.frame_idle);//帧间空闲功耗控制
	}
	else
	{
		acc_detector_presence_config_inter_frame_idle_state_set(presence_config, ACC_CONFIG_IDLE_STATE_DEEP_SLEEP);//帧间空闲功耗控制
	}
	acc_detector_presence_config_hwaas_set(presence_config, A121_presence_config.hwaas);
	acc_detector_presence_config_sweeps_per_frame_set(presence_config, A121_presence_config.sweeps_per_frame);

	acc_detector_presence_config_frame_rate_set(presence_config, A121_presence_config.frame_rate);

	acc_detector_presence_config_frame_rate_app_driven_set(presence_config, A121_presence_config.en_frame_rate);//帧频率受控设置
	acc_detector_presence_config_reset_filters_on_prepare_set(presence_config, A121_presence_config.en_reset_filters);//重置滤波器，清除累计数据
	//帧内数据配置
	acc_detector_presence_config_intra_detection_set(presence_config, A121_presence_config.en_intra_detec_flag);
	acc_detector_presence_config_intra_detection_threshold_set(presence_config, A121_presence_config.intra_threshold);
	acc_detector_presence_config_intra_frame_time_const_set(presence_config, A121_presence_config.intra_frame_time);//检测实时响应速度设置，响应慢减小，快加大
	acc_detector_presence_config_intra_output_time_const_set(presence_config, A121_presence_config.intra_output_time);//检测目标时信号不稳定则加大
	//帧间数据配置
	acc_detector_presence_config_inter_detection_set(presence_config, A121_presence_config.en_inter_detec_flag);
	acc_detector_presence_config_inter_detection_threshold_set(presence_config, A121_presence_config.inter_threshold);
	acc_detector_presence_config_inter_frame_deviation_time_const_set(presence_config, A121_presence_config.inter_deviation);
	acc_detector_presence_config_inter_frame_fast_cutoff_set(presence_config, A121_presence_config.inter_fast_cutoff);//快速检测滤波参数
	acc_detector_presence_config_inter_frame_slow_cutoff_set(presence_config, A121_presence_config.inter_slow_cutoff);//慢速检测滤波参数
	acc_detector_presence_config_inter_output_time_const_set(presence_config, A121_presence_config.inter_output_time);
	acc_detector_presence_config_inter_frame_presence_timeout_set(presence_config, A121_presence_config.inter_frame_presence_timeout);//快速丢掉历史数据

	acc_detector_presence_config_inter_phase_boost_set(presence_config, A121_presence_config.en_inter_phase_boost);//帧间相位检测，提高慢动作检测灵敏度


}



uint8_t define_config_mode(uint8_t mode)
{
	uint8_t set_flag=1;
	switch(mode)
	{
		case 1:
			A121_presence_config.start=0.06;
			A121_presence_config.end=1.5;
			A121_presence_config.step_length=0;//自动步长
			A121_presence_config.profile=0;//自适应profile
			A121_presence_config.frame_idle=ACC_CONFIG_IDLE_STATE_DEEP_SLEEP;
			A121_presence_config.hwaas=16;
			A121_presence_config.sweeps_per_frame=16;
			A121_presence_config.frame_rate=10;
			A121_presence_config.en_frame_rate=false;//帧频率受控设置
			A121_presence_config.en_reset_filters=true;

			A121_presence_config.en_intra_detec_flag=true;
			A121_presence_config.intra_threshold=1.4f;
			A121_presence_config.intra_frame_time=0.15f;//检测实时响应速度设置，响应慢减小，快加大
			A121_presence_config.intra_output_time=0.3f;//检测目标时信号不稳定则加大


			A121_presence_config.en_inter_detec_flag=true;
			A121_presence_config.inter_threshold=1.0f;
			A121_presence_config.inter_deviation=0.5f;
			A121_presence_config.inter_fast_cutoff=5.0f;//快速检测滤波参数
			A121_presence_config.inter_slow_cutoff=0.20f;//慢速检测滤波参数
			A121_presence_config.inter_output_time=2.0f;
			A121_presence_config.inter_frame_presence_timeout=3;//快速丢掉历史数据

			A121_presence_config.en_inter_phase_boost=false;//帧间相位检测，提高慢动作检测灵敏度
			break;
		case 2:
			A121_presence_config.start=0.3;
			A121_presence_config.end=3.0;
			A121_presence_config.step_length=0;//自动步长
			A121_presence_config.profile=0;//自适应profile
			A121_presence_config.frame_idle=ACC_CONFIG_IDLE_STATE_DEEP_SLEEP;
			A121_presence_config.hwaas=32;
			A121_presence_config.sweeps_per_frame=16;
			A121_presence_config.frame_rate=12;
			A121_presence_config.en_frame_rate=false;//帧频率受控设置
			A121_presence_config.en_reset_filters=true;

			A121_presence_config.en_intra_detec_flag=true;
			A121_presence_config.intra_threshold=1.3f;
			A121_presence_config.intra_frame_time=0.15f;//检测实时响应速度设置，响应慢减小，快加大
			A121_presence_config.intra_output_time=0.3f;//检测目标时信号不稳定则加大


			A121_presence_config.en_inter_detec_flag=true;
			A121_presence_config.inter_threshold=1.0f;
			A121_presence_config.inter_deviation=0.5f;
			A121_presence_config.inter_fast_cutoff=6.0f;//快速检测滤波参数
			A121_presence_config.inter_slow_cutoff=0.20f;//慢速检测滤波参数
			A121_presence_config.inter_output_time=2.0f;
			A121_presence_config.inter_frame_presence_timeout=3;//快速丢掉历史数据

			A121_presence_config.en_inter_phase_boost=false;//帧间相位检测，提高慢动作检测灵敏度
			break;
		case 3:
			A121_presence_config.start=3.0;
			A121_presence_config.end=8;
			A121_presence_config.step_length=0;//自动步长
			A121_presence_config.profile=0;//自适应profile
			A121_presence_config.frame_idle=ACC_CONFIG_IDLE_STATE_DEEP_SLEEP;
			A121_presence_config.hwaas=128;
			A121_presence_config.sweeps_per_frame=32;
			A121_presence_config.frame_rate=12;
			A121_presence_config.en_frame_rate=false;//帧频率受控设置
			A121_presence_config.en_reset_filters=true;

			A121_presence_config.en_intra_detec_flag=true;
			A121_presence_config.intra_threshold=1.2f;
			A121_presence_config.intra_frame_time=0.15f;//检测实时响应速度设置，响应慢减小，快加大
			A121_presence_config.intra_output_time=0.3f;//检测目标时信号不稳定则加大


			A121_presence_config.en_inter_detec_flag=true;
			A121_presence_config.inter_threshold=0.80f;
			A121_presence_config.inter_deviation=0.5f;
			A121_presence_config.inter_fast_cutoff=6.0f;//快速检测滤波参数
			A121_presence_config.inter_slow_cutoff=0.20f;//慢速检测滤波参数
			A121_presence_config.inter_output_time=2.0f;
			A121_presence_config.inter_frame_presence_timeout=3;//快速丢掉历史数据

			A121_presence_config.en_inter_phase_boost=false;//帧间相位检测，提高慢动作检测灵敏度
			break;
		default:set_flag=0;
			break;

	}
	return set_flag;
}











